﻿#include "MoveStage.h"
#include<QtDebug>
MoveStage::MoveStage(QString port_name)
{
  serial = new QSerialPort();

  serial->setBaudRate(9600);
  serial->setParity(QSerialPort::NoParity);
  serial->setDataBits(QSerialPort::Data8);
  serial->setStopBits(QSerialPort::OneStop);
  serial->setFlowControl(QSerialPort::NoFlowControl);
  _port_name =port_name;

  /*自动选择串口号，并打开设备*/
  serial->setPortName(_port_name);
  serial->open(QIODevice::ReadWrite);

  //connectDevice();
}

QString MoveStage::findDevice()
{
  QStringList port_list;

  std::cout<<"find port ..."<<std::endl;
  foreach (const QSerialPortInfo &info, QSerialPortInfo::availablePorts()) {
    serial->setPort(info);
    std::cout<<serial->portName().toStdString()<<"\n";
    if (!serial->open(QIODevice::ReadWrite)) {
      std::cout<<" fail serial->open(QIODevice::ReadWrite)"<<std::endl;
      continue;
    }
    if (!testDevice()) {
      std::cout<<" fail testDevice"<<std::endl;
      serial->close();
      continue;
    }
    std::cout<<" sucess "<<std::endl;
    port_list.append(serial->portName());
    serial->close();
    //find one break
    break;
  }
  if (port_list.empty())
    return "";
  return port_list[0];
}

bool MoveStage::testDevice()
{
  if (!serial->isOpen())
    return false;
  char * chars = new  char[10];
  chars[0]= 0x11;
  chars[1]= 0x11;
  chars[2]= 0x40;
  chars[3]= 0x01;
  chars[4]= 0x52;
  chars[5]= 0x00;
  chars[6]= 0x01;
  chars[7]= 0x00;
  chars[8]= 0x00;
  chars[9]= 0x00;
  QByteArray array(chars, 10);

  if (-1 == serial->write(array)){
    qDebug()<<"==testDevice==="<<"write fail";
    return false;
  }


  if (!serial->waitForReadyRead(1000)){
    qDebug()<<"==testDevice=="<<"waitForReadyRead";
    return false;
  }
  serial->readAll();
  //  QByteArray bytes = serial->readAll();
  //  qDebug()<<"testDevice  serial->readAll"<<" size :"<<bytes.size();
  //  for (int i = 0;i<bytes.size() ;i++){
  //    printf(" --   %x   \n", bytes.data()[i]&0xff);
  //  }
  //  if (0xA4 != (bytes.data()[0]&0xff))
  //    return false;

  return true;
}

bool MoveStage::connectDevice()
{
  if (serial->isOpen()) {
    serial->close();
  }

  if (!_port_name.isEmpty()) {
    serial->setPortName(_port_name);
    serial->open(QIODevice::ReadWrite);
    //打开了正确的设备
    if (testDevice()) {
      return true;
    }
    if (serial->isOpen())
      serial->close();

  }

  _port_name = findDevice();
  if (_port_name.isEmpty()){
    std::cout<<"port not found "<<"\n";
    return false;
  }
  serial->setPortName(_port_name);
  serial->open(QIODevice::ReadWrite);
  qDebug()<<"_port_name  found "<<_port_name<<" open \n";
  return true;
}

QString MoveStage::GetPortName()
{
  return  serial->portName();
}

void MoveStage::SetPortName(QString port_name)
{
  _port_name =port_name;
}

bool MoveStage::isConnect()
{
  return serial->isOpen();
}

bool MoveStage::setLR(unsigned int data_int)
{
  return writeCmd(0x44,data_int);
}
bool MoveStage::setSpeed(unsigned int data_int)
{
  return writeCmd(0x53,data_int);
}

bool MoveStage::setDistance(unsigned int data_int)
{
  return writeCmd(0x50,data_int);
}

bool MoveStage::lanchGo()
{
  return writeCmd(0x47,0x00);
}

bool MoveStage::writeCmd(char key, unsigned int data_int)
{
  char * chars = new  char[10];
  chars[0]= 0x11;chars[1]= 0x11;chars[2]= 0x40;
  //addr
  chars[3]= 0x01;
  chars[4]= key;
  chars[5]= 0x00;
  //data
  char * data = (  char *)&data_int; //unsigned int data_int

  chars[6]= data[0];
  chars[7]= data[1];
  chars[8]= data[2];
  chars[9]= data[3];

  QByteArray array(chars, 10);
  printf("write:\n");
  for (int i = 0;i<array.size() ;i++){
    printf("  %x  ", array.at(i)&0xff);
  }
  printf("\n");
  serial->write(array);


  if (!serial->waitForReadyRead(500)){
    printf("error waitForReadyRead:\n");
    return false;
  }


  return true;

}

